
#include "uskeleton.h"
#include <fstream>

USkeleton::USkeleton() {
   
   resetAuxiliarVars();
}

USkeleton::USkeleton(const char *filename) {
   
   ifstream skelfile(filename);
   if (not skelfile.is_open()) {
      cout<<"I/O error: grrr the skeleton file doesn't exist "<<endl;
      assert(false);
   }
   
   string skelformat; int njoints;
   // Load header
   skelfile>>skelformat;
   assert(skelformat=="SKE");
   skelfile>>njoints;
   
   // Load the joints positions
   Number x, y, z; int id, n;
   for (int i=0; i<njoints; ++i) {
      skelfile>>x>>y>>z;
      joints.push_back(Point3(x,y,z));
   }
   // Load the graph
   graph.resize(njoints);
   for (int i=0; i<njoints; ++i) {
      skelfile>>n;
      for (int j=0; j<n; ++j) {
         skelfile>>id;
         graph[i].push_back(id);
      }
   }
   
   // Generate the list of bones
   genBonesList();
   // Initialize auxiliar variables
   resetAuxiliarVars();
}

USkeleton::USkeleton(const vector<Point3> &j, const vector<vector<int> > &g) {
   
   joints = j;
   graph = g;
   
   // Generate the list of bones
   genBonesList();
   // Initialize auxiliar variables
   resetAuxiliarVars();
}

void USkeleton::genBonesList() {
   
   genBranches();
   
   /// bones in PREV format generation
   prev.clear();
   prev.resize(graph.size(), -1); // graph.size() == nJoints(), however nJoints() can't be present yet
   for (uint i=0; i<branches.size(); ++i) {
      const vector<int> &branch = branches[i];
      for (uint j=1; j<branch.size(); ++j) {
         const int &idprev = branch[j-1];
         const int &id = branch[j];
         prev[id] = idprev;
      }
   }
   
   bones.clear();
   /// bones in NORMAL format generation
   for (int i=0; i<prev.size(); ++i) {
      if (prev[i] < 0) continue;
      bones.push_back(make_pair(i, prev[i]));
   }
   
   cout<<"xthe bones ...."<<bones.size()<<endl;
   printBones();
   cout<<"xthe bones .... in prev format: "<<prev.size()<<endl;
   printBonesPREV();
   cout<<"xthe bones ...."<<branches.size()<<endl;
   printBranches();
   
   assert(prev.size() == bones.size()+1);
   
   /*
   //~ for (uint i=0; i<graph.size(); ++i) {
      //~ copy(graph[i].begin(), graph[i].end(), ostream_iterator<int>(cout, " "));
      //~ cout<<endl;
   //~ }
   
   // using a map to avoid duplicated keys on the bones list
   map<pair<int, int>, int > bonestmp;
   for (uint f=0; f<graph.size(); ++f) {
      const vector<int> &target = graph[f];
      for (uint c=0; c<target.size(); ++c) {
         uint cs = (f<target[c])? f : target[c];
         uint ct = (f<target[c])? target[c] : f;
         pair<int, int> pair(cs, ct);
         bonestmp.insert(make_pair(pair, 0));
      }
   }
   
   bones.clear();
   map<pair<int, int>, int >::iterator it = bonestmp.begin();
   while(it != bonestmp.end()) {
      const pair<int, int> &b = it->first;
      bones.push_back(b);
      ++it;
   }
   
   cout<<"lista de bones: "<<bones.size()<<endl;
   for (int i=0; i<bones.size(); ++i) {
      pair<int, int> &b = bones[i];
      cout<<b.first<<" : "<<b.second<<endl;
   }
   */
   
   relatedParam.resize(joints.size(), make_pair(0.0, -1));
   /*
   relatedParam[0].second = 18;
   relatedParam[1].second = 16;
   relatedParam[2].second = 15;
   relatedParam[3].second = 13;
   relatedParam[4].second = 17;
   relatedParam[5].second = 12;
   relatedParam[6].second = 6;
   relatedParam[7].second = 14;
   relatedParam[8].second = 19;
   relatedParam[9].second = 0;
   relatedParam[10].second = 4;
   relatedParam[11].second = 5;
   relatedParam[12].second = 2;
   relatedParam[13].second = 9;
   relatedParam[14].second = 3;
   relatedParam[15].second = 8;
   relatedParam[16].second = 10;*/
}

const Segment3 USkeleton::boneSegment(int i) const { 
   const pair<int, int> &bone = bones[i];
   const Point3 &a = joints[bone.first];
   const Point3 &b = joints[bone.second];
   return Segment3(a,b);
}

void USkeleton::export_ske(const char *fname) {
   
   string filename(fname);
   if (filename.empty()) return;
   if (filename.substr(filename.size() - 4, 4) != ".ske") { assert(false and "unknowed extension!"); }
   ofstream fileout(filename.c_str());
   if (fileout) {
      fileout<<"SKE"<<endl;
      fileout<<joints.size()<<endl;
      for (uint i=0; i<joints.size(); ++i)
         fileout<<joints[i]<<endl;
      
      for (uint i=0; i<graph.size(); ++i) {
         const vector<int> &neig = graph[i];
         fileout<<neig.size()<<" ";
         for (uint j=0; j<neig.size(); ++j)
            fileout<<neig[j]<<" ";
         fileout<<endl;
      }
   }
   fileout.close();
   
   // the skel-format has two parts:
   // (1) head
   //     SKE the skeleton header format identifier
   //     i: number_of_joints
   // (2) joins geometry (i entries)
   //     x y z: the joints positions
   // (3) graph connectivity (i entries)
   //     n id0 id1 id2 ... : {number of neighbors} {neighbors list}
   // ... example
   // SKEL
   // 3 
   // x0 y0 z0
   // x1 y1 z1
   // x2 y2 z2
   // 1 2
   // 1 2
   // 2 0 1
}

void USkeleton::export_skel(const char *fname) {
   
}

USkeleton* USkeleton::load(const char *filename) {
   
   std::fstream skel_file(filename, std::ios_base::in);
   if (skel_file) {
      USkeleton *s = new USkeleton(filename);
      return s;
   }
   
   assert(false and "loading failed!");
   return 0;
}

void USkeleton::translateJoint(int i, const Vector3 &t) {
   
   if (i<0 or i>=joints.size()) return;
   joints[i] = joints[i] + t;
}

int USkeleton::addJoint(const Point3 &p) {
   
   int id = joints.size();
   joints.push_back(p);
   vector<int> neighbords;
   graph.push_back(neighbords);
   return id;
}

void USkeleton::addBone(int i, int j) {
   
   if (i<0 or j<0) return;
   
   // dont insert duplicates
   if (find(graph[i].begin(), graph[i].end(), j) == graph[i].end())
      graph[i].push_back(j);
   if (find(graph[j].begin(), graph[j].end(), i) == graph[j].end())
      graph[j].push_back(i);
   // update the list of bones
   genBonesList();
}

Vector3 USkeleton::getBoneProjector(const Point3 &v, const int idbone) {
   
   const Segment3 &bo = boneSegment(idbone);
   const Point3 &a = bo.source();
   const Point3 &b = bo.target();
   
   const Vector3 &ba = b-a;
   const Vector3 &va = v-a;
   const Vector3 &vb = v-b;
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   if (dotvaibiai<=0)                  { return va; }
   else if (dotvaibiai>=norma2aibi)    { return vb; }
   else {
      Number t = dotvaibiai/norma2aibi;
      return (Vector3(v[0],v[1],v[2]) - (Vector3(b[0],b[1],b[2])*t + Vector3(a[0],a[1],a[2])*(1.0-t)));
   }
}

float USkeleton::getDistance(const Point3 &v, const int idbone, float t) {
   
   const Segment3 &bo = boneSegment(idbone);
   const Point3 &a = bo.source();
   const Point3 &b = bo.target();
   
   const Vector3 &ba = b-a;
   const Vector3 &va = v-a;
   const Vector3 &vb = v-b;
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   Number dminvaibi;
   if (dotvaibiai<=0)                  { dminvaibi = sqrt(va*va); t=1.0;}
   else if (dotvaibiai>=norma2aibi)    { dminvaibi = sqrt(vb*vb); t=0.0;}
   else                                { 
      dminvaibi = sqrt(va*va - ((dotvaibiai*dotvaibiai)/norma2aibi)); 
      t = 1.0-dotvaibiai/norma2aibi;
   }
   
   return dminvaibi;
}

void USkeleton::resetAuxiliarVars() {
   
   closest_vertex.clear();
   min_distance_vertex.clear();
   onlyvisible.clear();
   closest_vertex.resize(nJoints(), -1);
   min_distance_vertex.resize(nJoints(), 10e8);
   onlyvisible.resize(nJoints(), true);
}

void USkeleton::genBranches() {
   
   assert(graph.size() > 0);
   ismarked.resize(graph.size(), false);
   for (int i=0; i<graph.size(); ++i) {
      assert(not graph[i].empty());
      if (graph[i].size() < 3) continue;
      const vector<int> &neig = graph[i];
      for (uint j=0; j<neig.size(); ++j) {
         branch.clear();
         branch.push_back(i);
         ismarked[i] = true;
         getBranch(neig[j]);
         if(branch.size() > 1) branches.push_back(branch);
      }
   }
}

void USkeleton::getBranch(int id) {
   
   if (ismarked[id]) return;
   if (isTerminal(id) or isJunction(id)) {
      branch.push_back(id);
      ismarked[id] = true;
   } else {
      branch.push_back(id);
      ismarked[id] = true;
      const vector<int> &neig = graph[id];
      for (uint j=0; j<neig.size(); ++j) {
         if (ismarked[neig[j]]) continue;
         getBranch(neig[j]);
      }
   }
}

void USkeleton::printBones() {
   
   for (int i=0; i<bones.size(); ++i) {
      const pair<int, int> &bone = bones[i];
      cout<<i<<": "<<bone.first<<" "<<bone.second<<endl;
   }
}

void USkeleton::printBranches() {
   
   for (uint i=0; i<branches.size(); ++i) {
      cout<<i<<" : " ;
      copy(branches[i].begin(), branches[i].end(), ostream_iterator<int>(cout, " "));
      cout<<endl;
   }
}

void USkeleton::printBonesPREV() {
   
   for (uint i=0; i<prev.size(); ++i) {
      cout<<i<<" : "<<prev[i]<<endl;
   }
}
